#include <iostream>
#include <stdio.h>

#include <unistd.h>
#include <stdlib.h>

#include "dynamixel.h"

#include "dxl_hal.h"
#include "dxl_registers.h"
#include "Serial.hh"

using namespace DynamixelRX28;

#define DEFAULT_ID 1

class CommClass {
public:
	int tx( unsigned char *pPacket, int numPacket );
	int rx( unsigned char *pPacket, int numPacket );
	void set_timeout( int NumRcvByte );
	int timeout(void);
};

class Packet
{
public:
	enum PacketType {
		SetSpeed = 0,

	};
};



class HostClass {
public:
	void getPresentVoltage()
	{

	}
};

class TargetClass {

	int cached_isWheelmode;

	// Print communication result
	void printCommStatus(int commStatus)
	{
		switch(commStatus)
		{
		case COMM_TXFAIL:
			printf("COMM_TXFAIL: Failed transmit instruction packet!\n");
			break;

		case COMM_TXERROR:
			printf("COMM_TXERROR: Incorrect instruction packet!\n");
			break;

		case COMM_RXFAIL:
			printf("COMM_RXFAIL: Failed get status packet from device!\n");
			break;

		case COMM_RXWAITING:
			printf("COMM_RXWAITING: Now recieving status packet!\n");
			break;

		case COMM_RXTIMEOUT:
			printf("COMM_RXTIMEOUT: There is no status packet!\n");
			break;

		case COMM_RXCORRUPT:
			printf("COMM_RXCORRUPT: Incorrect status packet!\n");
			break;

		default:
			printf("This is unknown error code!\n");
			break;
		}
	}

	// Print error bit of status packet
	void printErrorCode()
	{
		if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
			printf("Input voltage error!\n");

		if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
			printf("Angle limit error!\n");

		if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
			printf("Overheat error!\n");

		if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
			printf("Out of range error!\n");

		if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
			printf("Checksum error!\n");

		if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
			printf("Overload error!\n");

		if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
			printf("Instruction code error!\n");
	}

	/** @return -1 on error, 1 in wheel mode, 0 in absolute mode */
	int isWheelMode_uncached() const
	{
		int cw_angle_limit = dxl_read_word( DEFAULT_ID, P_CW_ANGLE_LIMIT_L );
		int commStatus = dxl_get_result();
		if (commStatus == COMM_RXSUCCESS)
		{
			int ccw_angle_limit = dxl_read_word( DEFAULT_ID, P_CW_ANGLE_LIMIT_L );
			commStatus = dxl_get_result();
			if (commStatus == COMM_RXSUCCESS)
			{
				bool state = ((cw_angle_limit == 0) && (ccw_angle_limit == 0));

				return state;
			}
		}

		return -1;
	}

	void retrying_word_write(int id, int reg, int value)
	{
		int commStatus;

		do
		{
			dxl_write_word( id, reg, value );
			commStatus = dxl_get_result();
			if( commStatus != COMM_RXSUCCESS )
			{
				printCommStatus(commStatus);
			}
		} while (commStatus != COMM_RXSUCCESS);
	}

public:
	TargetClass()
	{
		cached_isWheelmode = -1;
	}

	/** @return -1 on error, otherwise 1 when in wheel mode */
	int isWheelMode()
	{
		if (cached_isWheelmode == -1)
			cached_isWheelmode = isWheelMode_uncached();

		return cached_isWheelmode;
	}

	/** @return -1 on error */
	int setTorqueEnable(bool on)
	{
		dxl_write_byte( DEFAULT_ID, P_TORQUE_ENABLE, on?1:0 );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return 0;
		}else{
			printCommStatus(commStatus);
			return -1;
		}
	}

	/** @param pos between 0 and 1023 */
	int setGoalPosition(int pos)
	{
		dxl_write_word( DEFAULT_ID, P_GOAL_POSITION_L, pos );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return 0;
		}else{
			printCommStatus(commStatus);
			return -1;
		}
	}

	int getPresentPosition()
	{
		// Read present position
		int pos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return pos;
		}
		else
		{
			printCommStatus(commStatus);
			return -1;
		}
	}

	int getPresentSpeed()
	{
		// Read present speed
		int speed = dxl_read_word( DEFAULT_ID, P_PRESENT_SPEED_L );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return speed;
		}
		else
		{
			printCommStatus(commStatus);
			return -1;
		}
	}

	int getPresentLoad()
	{
		// Read present load
		int speed = dxl_read_word( DEFAULT_ID, P_PRESENT_LOAD_L );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return speed;
		}
		else
		{
			printCommStatus(commStatus);
			return -1;
		}
	}


	/** @return Voltage * 10, or -1 on error */
	int getPresentVoltage()
	{
		int voltage = dxl_read_byte( DEFAULT_ID, P_PRESENT_VOLTAGE );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return voltage;
		}
		else
		{
			printCommStatus(commStatus);
			return -1;
		}
	}

	/** @return Temperature in degrees, or -1 on error */
	int getPresentTemperature()
	{
		int temperature = dxl_read_byte( DEFAULT_ID, P_PRESENT_TEMPERATURE );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return temperature;
		}
		else
		{
			printCommStatus(commStatus);
			return -1;
		}
	}


	int isMoving()
	{
		int moving = dxl_read_byte( DEFAULT_ID, P_MOVING );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return moving;
		}
		else
		{
			printCommStatus(commStatus);
			return -1;
		}
	}

	int setLED(bool on)
	{
		dxl_write_byte( DEFAULT_ID, P_LED, on?1:0 );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return 0;
		}else{
			printCommStatus(commStatus);
			return -1;
		}
	}

	/** @param isCW true for CW rotation
	 * @param torque Torque to use. In the range of */
	int setWheelModeTorque(bool isCW, int torque)
	{
		int word = (isCW?1024:0) | (torque & 0x3FF);
		dxl_write_word( DEFAULT_ID, P_GOAL_SPEED_L, word );
		int commStatus = dxl_get_result();
		if( commStatus == COMM_RXSUCCESS )
		{
			return 0;
		}else{
			printCommStatus(commStatus);
			return -1;
		}
	}


	int setWheelMode(bool enabled = 1)
	{
		if (enabled)
		{
			retrying_word_write(DEFAULT_ID, P_CW_ANGLE_LIMIT_L, 0);
			retrying_word_write(DEFAULT_ID, P_CCW_ANGLE_LIMIT_L, 0);
		} else {
			retrying_word_write(DEFAULT_ID, P_CW_ANGLE_LIMIT_L, 0);
			retrying_word_write(DEFAULT_ID, P_CCW_ANGLE_LIMIT_L, 1023);
		}
		return 0;
	}

	void dumpAllRegisters()
	{
		for(int i = 0; i <= 49; i++)
		{
			int commStatus;
			do{
				int val = dxl_read_byte( DEFAULT_ID, i );
				commStatus = dxl_get_result();

				if( commStatus == COMM_RXSUCCESS )
				{
					std::cout << i << "; " << val << std::endl;
				}
				else
				{
					printCommStatus(commStatus);
				}
			}while(commStatus != COMM_RXSUCCESS);
		}
	}

};


const char* g_devname = "/dev/ttyUSB1";

void setSpeed(double speed)
{
	std::cout << "NOT IMPLEMENTED!";
}

/**Sleep for specified number of milliseconds. Returns earlier only if sleeping process interrupted. */
void msleep(int delay)
{
	  timespec ts;
	  ts.tv_sec=delay/1000;;
	  ts.tv_nsec=(delay%1000)*1000;
	  nanosleep(&ts,0);
}

void report(TargetClass &t)
{
	std::cout << "pos=" << t.getPresentPosition() << ", speed=" << t.getPresentSpeed() << ", load=" << t.getPresentLoad() << ", T=" << t.getPresentTemperature() << "C, V=" << t.getPresentVoltage()*0.1 << "\n";
}

void report2(TargetClass &t)
{
	std::cout << "pos=" << t.getPresentPosition() << ", speed=" << t.getPresentSpeed() << ", load=" << t.getPresentLoad() << "\n";
}

int main(int argc, char* argv[])
{

	Serial serial(g_devname);
	dxl_hal_open(serial, 57600);
	TargetClass t;

	char opt;
	while ((opt = getopt(argc, argv, "as:d:rpw")) != -1)
	{
		switch (opt)
		{
		case 'w':
		{
			t.setWheelMode(1);
			msleep(20);
			while(t.setTorqueEnable(1) == -1);


			while(t.setWheelModeTorque(0,1023)==-1);
			for(int i = 0; i < 1000; i++)
			{
				report2(t);
				msleep(2);
			}

			/*
			for(int force = 0; force < 1023; force+=100)
			{
				std::cout << "Setting wheel mode force to " << force << "\n";
				while(t.setWheelModeTorque(1,force)==-1);

				for(int i = 0; i < 10; i++)
				{
					report(t);
					msleep(2);
				}

			}
			*/



			while(t.setWheelModeTorque(1,0)==-1);

			while(t.setTorqueEnable(0)==-1);
			//t.setWheelMode(0);
			break;
		}
		case 'a':
		{
			t.dumpAllRegisters();
			break;
		}
		case 'p':
		{
			//Pan back and forth...
			t.setTorqueEnable(1);

			t.setGoalPosition(0);
			msleep(1000);
			t.setGoalPosition(1023);
			while(t.isMoving())
			{
				msleep(20);
				report(t);
				msleep(20);
			}
			msleep(1000);
			t.setGoalPosition(0);

			t.setTorqueEnable(0);
			break;

		}
		case 'r':
		{
			//report
			report(t);
			break;
		}
		break;
		case 'd':
		{
			char* endptr;
			double delay = strtod(optarg, &endptr);
			if(delay==0 && (endptr == optarg))
			{
				std::cerr << "Unable to parse -d <number>\n";
			}
			else
			{
				msleep(delay);
			}
		}
			break;

		case 's':
		{
			char* endptr;
			double speed = strtod(optarg, &endptr);
			if(speed==0 && (endptr == optarg))
			{
				std::cerr << "Unable to parse -s <number>\n";
			}
			else
			{
				setSpeed(speed);
			}
		}
			break;

		default: /* '?' */
			std::cerr << "Usage: " << argv[0] << " [-s speed] [-d msec_delay]" << std::endl;
			exit(1);
		}
	}
	return 0;
}
